/*
 * Maze.cpp
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#include "Maze.h"

namespace std {

Maze::Maze(int date[],int h, int w) : height(h),width(w),update_cnt(0),data(),beacons(),blocks(),img(NULL),imageShown(false), wDiff(PIC_WIDTH / width),hDiff(PIC_WIDTH / height) {
	double nb_y = 0;
//	data.resize(10, vector<int>());
//	for (int i = 0; i < 10; i++) {
//		data[i].resize(10, 0);
//		for (int j = 0; j < 10; j++) {
//			data[i][j] = i * j;
//		}
//	}
//	for (int i = 0; i < h; i++) {
//		for (int j = 0; j < h; j++) {
//			cout << (data[i][j]) << " ";
//		}
//		cout << endl;
//	}
//	cout << endl;
//	cout <<"works?" << endl;
//	Point p(10,10);
//	cout << "wth!"<<endl;
//
//	cout <<"works? 2" << endl;
//	Point q ( p );
//	cout << "wth!"<<endl;
//
////	blocks.insert(blocks.begin(),q);
////	cout <<"works .. 2" << endl;
//	blocks.push_back(q);
//	cout <<"works .. 2!!!" << endl;
//
//	for(int i = 0; i < h; i++){
//		Point p (i,i);
//		cout << (p.x) << " " << (p.y) << endl;
//		blocks.push_back(p);
//		Point q(p);
//	}
//	cout << endl;
//	for (int i = 0; i < h; i++) {
//		cout << (blocks[i].x) << " " << (blocks[i].y) << endl;
//	}

	this->data.resize(width,vector<int>());
	for(int j = 0; j < width; j++){
		this->data[j].resize(height,0);
		for(int i = 0; i < height; i++){
			this->data[j][i] = date[j*width+i];
			if(date[j*width+i] > 0){
				nb_y = height - i - 1;
				blocks.push_back(Point(j,nb_y));
				if(date[j*width+i] == 2){
					beacons.push_back(Point(j,nb_y));
					beacons.push_back(Point(j+1,nb_y));
					beacons.push_back(Point(j,nb_y+1));
					beacons.push_back(Point(j+1,nb_y+1));
				}
			}
		}
	}
}

Maze::~Maze() {
//	releaseImage();
	cout << "fucked up!" << endl;
}

void Maze::releaseImage(){
	if(imageShown){
		cvReleaseImage(&img);
		cvDestroyWindow("Maze");
	}
}

Point Maze::random_place(){
	return Point(UNIFORM_RAND_WITHOUT_ONE*width,UNIFORM_RAND_WITHOUT_ONE*height);
}

Point Maze::random_free_place(){
	while(true){
//		cout << "lets find a free point" << endl;
		Point p = random_place();
//		cout << p.x << " " << p.y <<" "<< height<<" " << width << endl;
		if(is_free(p)){
//			cout << "Point is free!" << endl;
			return Point(p);
		}
	}
//	cout << "no Point found" <<endl;
	return Point(-1,-1);
}

void Maze::draw(){
	imageShown = true;
	/*********************************/
	img = cvCreateImage(cvSize(PIC_WIDTH, PIC_HEIGHT), IPL_DEPTH_8U, 3);
	if(img == NULL){cout << "img == null" <<endl; return;}
	cvNamedWindow("Maze", CV_WINDOW_AUTOSIZE);
	char *ptrD = img->imageData;
	/*********************************/
	for (int y = 0; y < PIC_HEIGHT; y++) {
		for (int x = 0; x < PIC_WIDTH; x++) {
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;
		}
	}
	cvShowImage("Maze" , img);
	cvWaitKey(80);
}

void Maze::redraw(){
	if(img->imageData == NULL) cout << "error! AAAARGH!"<<endl;
	char *ptrD = img->imageData;
	/*********************************/
	for (int y = 0; y < PIC_HEIGHT; y++) {
		for (int x = 0; x < PIC_WIDTH; x++) {
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;
			*(ptrD++) = 0xFF;
		}
	}
	for (int i = 0; i < width; i++) {
		for (int j = 0; j < height; j++) {
			switch (this->data[j][i]) {
			case 2:
				cvRectangle(img, cvPoint(i * wDiff, j * hDiff),
						cvPoint((i + 1) * wDiff, (j + 1) * hDiff),
						cvScalar(0, 0, 0), CV_FILLED, 1, 0);
				cvCircle(img, cvPoint(i * wDiff, j * hDiff), 3,
						cvScalar(0, 0, 0xFF), 3);
				cvCircle(img, cvPoint((i + 1) * wDiff, j * hDiff), 3,
						cvScalar(0, 0, 0xFF), 3);
				cvCircle(img, cvPoint(i * wDiff, (j + 1) * hDiff), 3,
						cvScalar(0, 0, 0xFF), 3);
				cvCircle(img, cvPoint((i + 1) * wDiff, (j + 1) * hDiff), 3,
						cvScalar(0, 0, 0xFF), 3);
				break;
			case 1:
				cvRectangle(img, cvPoint(i * wDiff, j * hDiff),
						cvPoint((i + 1) * wDiff, (j + 1) * hDiff),
						cvScalar(0, 0, 0), CV_FILLED, 1, 0);
				break;
			default:
				break;
			}
		}
	}
}

void Maze::drawRobot(Point r){
	if(r.x >=0 && r.y >= 0 && r.x < height && r.y < width ){
		cvCircle(img, cvPoint(r.x * wDiff, PIC_HEIGHT - r.y * hDiff), 3, cvScalar(0xFF, 0, 0), 3);
	}
}

void Maze::repaint(){
	cvShowImage("Maze", img);
	cvWaitKey(10);
}

void Maze::drawParticles(Point p){
	if(p.x >=0 && p.y >= 0 && p.x < height && p.y < width ){
		cvCircle(img, cvPoint(p.x * wDiff, PIC_HEIGHT - p.y * hDiff), 2, cvScalar(0, 0xFF, 0), 0);
	}
}

bool Maze::is_in(Point p){
	return !(p.x < 0 || p.y < 0 || p.x > this->width || p.y > this->height);
}

double Maze::distance(Point p1, Point p2){
	return sqrt((p1.x - p2.x)*(p1.x - p2.x)+(p1.y - p2.y)*(p1.y - p2.y));
}

double Maze::distance_to_nearest_beacon(Point p){
	double d = 9999;
//	double d2 = 9999;
	for(unsigned int i = 0; i < beacons.size(); i++){
		double dist = distance(p,beacons[i]);
		if(dist < d) d = dist;
//		if(dist > d && dist < d2) d2 = dist;
	}
//	return sqrt(d*d + d2*d2);
	return d;
}

bool Maze::is_free(Point p){
	if(!is_in(p)) return false;
	int x_co = (int)p.x;
	int y_co = height - (int)p.y - 1;
	return data[y_co][x_co]==0;
}

} /* namespace std */
